"""
运动控制服务器
提供机器人运动控制相关的MCP工具
"""

from typing import Dict, Any, List
from mcp.server.fastmcp import FastMCP
from robot_core.action_executor import action_executor


class MotionServer(FastMCP):
    """运动控制服务器类"""

    def __init__(self):
        """初始化运动控制服务器"""
        super().__init__("motion_server")

        # 启动动作执行线程
        action_executor.start_action_thread()

        # 注册工具
        self._register_tools()

    def _register_tools(self):
        """注册所有运动控制工具"""

        @self.tool()
        async def run_action(action_name: str, times: int = 1) -> Dict[str, Any]:
            """执行指定动作组

            Args:
                action_name: 动作组名称
                times: 执行次数，0表示无限循环

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                action_executor.change_action(action_name, times)
                return {
                    "status": "success",
                    "message": f"Running action {action_name} for {times} times",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def run_action_sequence(
            action_codes: List[str], times: int = 1
        ) -> Dict[str, Any]:
            """执行动作序列

            Args:
                action_codes: 动作代码列表
                times: 整个序列重复执行的次数

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                action_executor.run_action_sequence(action_codes, times)
                return {
                    "status": "success",
                    "message": f"Running action sequence for {times} times",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def stop_action() -> Dict[str, Any]:
            """停止当前动作执行

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                action_executor.stop_action_group()
                return {"status": "success", "message": "Stopped current action"}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def is_action_finished() -> Dict[str, Any]:
            """检查动作是否执行完成

            Returns:
                Dict[str, Any]: 执行状态
            """
            try:
                finished = action_executor.is_action_finished()
                return {"status": "success", "finished": finished}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def set_servo_position(
            servo_id: int, position: int, duration: int = 1000
        ) -> Dict[str, Any]:
            """设置单个舵机位置

            Args:
                servo_id: 舵机ID
                position: 目标位置(0-1000)
                duration: 运行时间(10-30000ms)

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                action_executor.set_servo(servo_id, position, duration)
                return {
                    "status": "success",
                    "message": f"Set servo {servo_id} to position {position} in {duration}ms",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def stop_servo(servo_id: int = None) -> Dict[str, Any]:
            """停止舵机运行

            Args:
                servo_id: 舵机ID，如果为None则停止所有舵机

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                action_executor.stop_servo(servo_id)
                return {
                    "status": "success",
                    "message": f"Stopped servo {servo_id if servo_id else 'all'}",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def move_forward(speed: float = 0.5) -> Dict[str, Any]:
            """控制机器人向前移动

            Args:
                speed: 移动速度,范围0-1

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                action_executor.change_action("forward", 0)  # 0表示无限循环
                return {
                    "status": "success",
                    "message": f"Moving forward at speed {speed}",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def move_backward(speed: float = 0.5) -> Dict[str, Any]:
            """控制机器人向后移动

            Args:
                speed: 移动速度,范围0-1

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                action_executor.change_action("backward", 0)
                return {
                    "status": "success",
                    "message": f"Moving backward at speed {speed}",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def turn_left(angle: float = 90) -> Dict[str, Any]:
            """控制机器人向左转

            Args:
                angle: 旋转角度,单位度

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                action_executor.change_action("turn_left", 1)
                return {"status": "success", "message": f"Turning left {angle} degrees"}
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def turn_right(angle: float = 90) -> Dict[str, Any]:
            """控制机器人向右转

            Args:
                angle: 旋转角度,单位度

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                action_executor.change_action("turn_right", 1)
                return {
                    "status": "success",
                    "message": f"Turning right {angle} degrees",
                }
            except Exception as e:
                return {"status": "error", "message": str(e)}

        @self.tool()
        async def stop() -> Dict[str, Any]:
            """停止机器人运动

            Returns:
                Dict[str, Any]: 执行结果
            """
            try:
                action_executor.stop_action_group()
                return {"status": "success", "message": "Stopped all motion"}
            except Exception as e:
                return {"status": "error", "message": str(e)}
